use std::f64::consts::PI;

use glam::{
    f64::{DAffine3, DQuat, DVec3},
    EulerRot,
};

pub fn c2n_timestamp(timestamp: u64) -> u64 {
    timestamp
}

pub fn c2n_location(xyz: (f32, f32, f32)) -> (f64, f64, f64) {
    (xyz.0 as f64, -xyz.1 as f64, xyz.2 as f64)
}

pub fn c2n_rotation(pyr: (f32, f32, f32)) -> (f64, f64, f64, f64) {
    let q = DQuat::from_euler(
        EulerRot::YZX,
        (pyr.0 as f64) * PI / 180.0,
        -(pyr.1 as f64) * PI / 180.0,
        (pyr.2 as f64) * PI / 180.0,
    );

    (q.w, q.x, q.y, q.z)
}

pub fn c2n_vector(xyz: (f32, f32, f32)) -> (f64, f64, f64) {
    (xyz.0 as f64, -xyz.1 as f64, xyz.2 as f64)
}

pub fn c2n_angles(xyz: (f32, f32, f32)) -> (f64, f64, f64) {
    (
        (xyz.0 as f64) * PI / 180.0,
        -(xyz.1 as f64) * PI / 180.0,
        (xyz.2 as f64) * PI / 180.0,
    )
}

pub fn n2c_location(xyz: (f64, f64, f64)) -> (f32, f32, f32) {
    (xyz.0 as f32, -xyz.1 as f32, xyz.2 as f32)
}

pub fn n2c_rotation(wxyz: (f64, f64, f64, f64)) -> (f32, f32, f32) {
    let pyr = DQuat::from_xyzw(wxyz.1, wxyz.2, wxyz.3, wxyz.0).to_euler(glam::EulerRot::YZX);

    (
        (pyr.0 * 180.0 / PI) as f32,
        -(pyr.1 * 180.0 / PI) as f32,
        (pyr.2 * 180.0 / PI) as f32,
    )
}

pub struct Transform {
    tf: DAffine3,
}

impl Transform {
    pub fn from_base_point(
        position_xyz: (f64, f64, f64),
        orientation_wxyz: (f64, f64, f64, f64),
    ) -> Self {
        let r = DQuat::from_xyzw(
            orientation_wxyz.1,
            orientation_wxyz.2,
            orientation_wxyz.3,
            orientation_wxyz.0,
        )
        .inverse();
        let t = r * -DVec3::new(position_xyz.0, position_xyz.1, position_xyz.2);

        Self {
            tf: DAffine3::from_rotation_translation(r, t),
        }
    }

    pub fn inverse(&self) -> Self {
        Self {
            tf: self.tf.inverse(),
        }
    }

    pub fn transform(
        &self,
        position_xyz: (f64, f64, f64),
        orientation_wxyz: (f64, f64, f64, f64),
    ) -> ((f64, f64, f64), (f64, f64, f64, f64)) {
        let p =
            self.tf
                .transform_point3(DVec3::new(position_xyz.0, position_xyz.1, position_xyz.2));
        let o = DQuat::from_mat3(&self.tf.matrix3)
            * DQuat::from_xyzw(
                orientation_wxyz.1,
                orientation_wxyz.2,
                orientation_wxyz.3,
                orientation_wxyz.0,
            );

        ((p.x, p.y, p.z), (o.w, o.x, o.y, o.z))
    }

    pub fn transform_vector(&self, vector_xyz: (f64, f64, f64)) -> (f64, f64, f64) {
        let v = self
            .tf
            .transform_vector3(DVec3::new(vector_xyz.0, vector_xyz.1, vector_xyz.2));

        (v.x, v.y, v.z)
    }
}
